
public class FrameMotionInfo {
	final static private int BLOCK_SIZE = 16;
	final static private int SEARCH_PARAMETER = 7;
	static private int WIDTH = 320;
	static private int HEIGHT = 240;
	static private int BLOCK_COUNT = (WIDTH/BLOCK_SIZE) * (HEIGHT/BLOCK_SIZE);
	static int[] SDSP_X = {0, -1,  0, 1, 0};
	static int[] SDSP_Y = {0,  0, -1, 0, 1};
	
	static public void setSize(int width, int height){
		WIDTH = width;
		HEIGHT = height;
		BLOCK_COUNT = (WIDTH/BLOCK_SIZE) * (HEIGHT/BLOCK_SIZE);
	}
	
	//Member properties
	public MotionVector[] vectors;
	public double modSum;
	private int[][] selfImage;
	private int[][] refImage;
	
	public FrameMotionInfo(){
		vectors = new MotionVector[BLOCK_COUNT];
		for(int i=0; i<vectors.length; ++i){
			vectors[i] = new MotionVector();
		}
	}
	
	public void print(){
		System.out.println("++++++++++++++++++++++++++++++++++++++++++++++++++");
		int count = 0;
		int blockInRow = WIDTH/BLOCK_SIZE;
		for(int i=0; i<BLOCK_COUNT; ++i){
			System.out.print("x=" + count + " y=" + i/blockInRow + "(" + vectors[i].x + "," + vectors[i].y + ") ");
			if(++count == blockInRow){
				count = 0;
				System.out.println();
			}
		}
	}
	
	public double calMotionSum(){
		modSum = 0;
		for(int i=0; i<vectors.length; i++){
			modSum += vectors[i].x * vectors[i].x + vectors[i].y * vectors[i].y;
		}
		modSum /= vectors.length;
		return modSum;
	}
	
	/***************************************************
	 * Calculate the cost between blocks
	 */
	public int calcBlockMAD(int selfX, int selfY, int refX, int refY){
		int sum = 0;
		for(int i=0; i<BLOCK_SIZE; ++i){
			for(int j=0; j<BLOCK_SIZE; ++j){
				sum += Math.abs(selfImage[selfY+i][selfX+j] - refImage[refY+i][refX+j]);
			}
		}
		return sum;
	}
	
	
	/***************************************************
	 * Calculate the motion vectors of a single frame
	 * Using Brute Force method
	 */
	public void calcVectorsUsingBruteForce(int[][] originalData, int[][] referenceData){
		
		int blockRows = originalData.length/BLOCK_SIZE;
		int blockColumns = originalData[0].length/BLOCK_SIZE;
		selfImage = originalData;
		refImage = referenceData;
		for(int by=0; by < blockRows; by++){
			for(int bx=0; bx < blockColumns; bx++){
				int blockX = bx * BLOCK_SIZE; // upper left point of block
				int blockY = by * BLOCK_SIZE; // upper left point of block
				
				if( (blockX - SEARCH_PARAMETER < 0) ||
					(blockX + BLOCK_SIZE + SEARCH_PARAMETER > WIDTH) ||
					(blockY - SEARCH_PARAMETER < 0) ||
					(blockY + BLOCK_SIZE + SEARCH_PARAMETER > HEIGHT))
				{
						vectors[by * blockColumns + bx].x = 0;
						vectors[by * blockColumns + bx].y = 0;
				}
				else
				{
					int ymin=0, xmin=0;
					int min=9999999;
					for(int refY = blockY-SEARCH_PARAMETER; refY <= blockY+SEARCH_PARAMETER; refY++)
					{	
						for(int refX = blockX-SEARCH_PARAMETER; refX <= blockX+SEARCH_PARAMETER; refX++)
						{
							// calculate block difference
							int sum = this.calcBlockMAD(blockX, blockY, refX, refY);
							if(sum<min)
							{
								min=sum;
								ymin = refY;//-blockY;
								xmin = refX;//-blockX;
							}
						}
					}
					
					vectors[by * blockColumns + bx].x = xmin - blockX;
					vectors[by * blockColumns + bx].y = ymin - blockY;

				}
			}
		}
		selfImage = null;
		refImage = null;
	}
	
	/***************************************************
	 * Calculate the motion vectors of a single frame
	 * Using ARPS method
	 */
	public void calcVectorsUsingARPS(int[][] originalData, int[][] referenceData){
		int[][] checkMatrix = new int[2*SEARCH_PARAMETER+1][2*SEARCH_PARAMETER+1];
		int[] costs = {Integer.MAX_VALUE, Integer.MAX_VALUE, Integer.MAX_VALUE, 
				Integer.MAX_VALUE, Integer.MAX_VALUE, Integer.MAX_VALUE};
		int mbCount = 0;
		int stepSize;
		int maxIndex;
		selfImage = originalData;
		refImage = referenceData;
		for (int i=0; i<originalData.length; i+=BLOCK_SIZE){
			for(int j=0; j<originalData[0].length; j+=BLOCK_SIZE){
				//Calculate SDSP(0,0) first
				costs[0] = calcBlockMAD(j, i, j, i);
				checkMatrix[SEARCH_PARAMETER][SEARCH_PARAMETER] = 1;
				
				//If we are in the left most column then we have to make sure that
				//we just do the LDSP with stepSize = 2
				if (j==0){
					stepSize = 2;
					maxIndex = 5;
				}else{
					stepSize = Math.max(Math.abs(vectors[mbCount-1].x),
							Math.abs(vectors[mbCount-1].y));
					if (stepSize == 0)
						stepSize = 2;
					//Now we have to make sure that if the point due to motion vector
					//is one of the LDSP points then we don't calculate it again
					if(vectors[mbCount-1].x==0 || vectors[mbCount-1].y==0){
						maxIndex = 5; //we just have to check at the rood pattern
					}else{
						maxIndex = 6;
					}
				}
				//Large diamond search pattern
				int[] LDSP_X = new int[maxIndex];
				int[] LDSP_Y = new int[maxIndex];
				LDSP_X[0] = 0;           LDSP_Y[0] = 0;
				LDSP_X[1] = -stepSize;   LDSP_Y[1] = 0;
				LDSP_X[2] = 0;           LDSP_Y[2] = -stepSize;
				LDSP_X[3] = stepSize;    LDSP_Y[3] = 0;
				LDSP_X[4] = 0;           LDSP_Y[4] = stepSize;
				if(maxIndex>5){
					LDSP_X[5] = vectors[mbCount-1].x;
					LDSP_Y[5] = vectors[mbCount-1].y;
				}
				
				int x = j;
				int y = i;
				
				//Do the LDSP
				//If stepSize == 0, all LDSP lie in the center, center point already calculated
				for (int k=1; k<LDSP_X.length; ++k){
					int refY = y + LDSP_Y[k];
					int refX = x + LDSP_X[k];
					if(refY < 0 || refY >= originalData.length-BLOCK_SIZE || 
							refX < 0 || refX >= originalData[0].length-BLOCK_SIZE){
						continue;  //Outside image boundary
					}					
					costs[k] = calcBlockMAD(j, i, refX, refY);
					checkMatrix[SEARCH_PARAMETER+LDSP_Y[k]][SEARCH_PARAMETER+LDSP_X[k]] = 1;
				}
				
				int minK = -1;
				int minCost = Integer.MAX_VALUE;
				for(int k=0; k<costs.length; ++k){
					if (minCost > costs[k]){
						minCost = costs[k];
						minK = k;
					}
					costs[k] = Integer.MAX_VALUE;
				}
				x += LDSP_X[minK];
				y += LDSP_Y[minK];
				costs[0] = minCost;
				
				//The continueFlag is set to false when the minimum is at the center of the diamond
				boolean continueFlag = true;
				while(continueFlag){
					//Do the SDSP
					int refY;
					int refX;
					for(int k=1; k<5; ++k){
						refY = y+SDSP_Y[k];
						refX = x+SDSP_X[k];
						if(refY < 0 || refY >= originalData.length-BLOCK_SIZE || 
								refX < 0 || refX >= originalData[0].length-BLOCK_SIZE ||
								refX < j-SEARCH_PARAMETER || refX > j+SEARCH_PARAMETER ||
								refY < i-SEARCH_PARAMETER || refY > i+SEARCH_PARAMETER){
							continue;  //Outside image boundary
						}else if(checkMatrix[y-i+SDSP_Y[k]+SEARCH_PARAMETER][x-j+SDSP_X[k]+SEARCH_PARAMETER] == 1){
							continue;
						}
						
						costs[k] = calcBlockMAD(j, i, refX, refY);
						checkMatrix[y-i+SDSP_Y[k]+SEARCH_PARAMETER][x-j+SDSP_X[k]+SEARCH_PARAMETER] = 1;
					}
					
					minK = -1;
					minCost = Integer.MAX_VALUE;
					for(int k=0; k<5; ++k){
						if (minCost > costs[k]){
							minCost = costs[k];
							minK = k;
						}
						costs[k] = Integer.MAX_VALUE;
					}
					if(minK == 0){
						continueFlag = false;
					}else{
						x += SDSP_X[minK];
						y += SDSP_Y[minK];
						costs[0] = minCost;
					}
				}
				
				vectors[mbCount].x = x - j;
				vectors[mbCount].y = y - i;
				++mbCount;
				
				for(int s=0; s<checkMatrix.length; ++s){
					for(int r=0; r<checkMatrix[0].length; ++r){
						checkMatrix[s][r] = 0;
					}
				}
			}
		}
		selfImage = null;
		refImage = null;
	}
	
	
}
